//
// Created by xuwj on 2021/10/29.
//

#include <iostream>

#include "opencv2/core/core.hpp"
#include "opencv2/opencv.hpp"

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"

#include "visual_frames_system.h"

using namespace std;
using namespace VNavAssistant;

const string file_path = "/home/xuwj/other_workspaces/visualNav_ws/src/vnav_assistant/configs/visual_frames_helper.yaml";

void SLAMPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg){

}

int main(int argc, char **argv){
    ros::init(argc,argv,"visual_tf_manager");
    ros::NodeHandle nh;

    //Load FrameID
    cv::FileStorage fSettings(file_path,CV_STORAGE_READ);
    const string robot_base_frame = fSettings["RobotBaseFrame"];
    const string map_frame = fSettings["MapFrame"];
    const string odom_frame = fSettings["OdomRobotFrame"];
    const string camera_frame = fSettings["CameraFrame"];

    //Load fixed transformations between frames
    cv::Mat base2cam;
    fSettings["Trc"] >> base2cam;

    //Load Working Mode and Topics
    const int work_mode = fSettings["Mode"];

    // mode1下， 定位算法自己处理tag_odom，并发布出Tcam0_camk
    if(work_mode == 1){
        cout<<" Working on Mode 1.\n\n";
        const string slam_pose_topic = fSettings["SLAMPoseTopic"];
        PoseRectifySystem* pPoseRectifySystem = new PoseRectifySystem(camera_frame,odom_frame);
        //Create topic subscriber
        ros::Subscriber slam_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(slam_pose_topic,5,bind(&PoseRectifySystem::SLAMPoseCallback,pPoseRectifySystem,_1));
    }

    else if(work_mode == 0){
        cout<<"Working on Mode 0.\n\n";
        const string tag_topic = fSettings["AprilTagTopic"];
        const int tag_buff_size = fSettings["TagBuffSize"];
        const string initial_pose_topic = fSettings["InitialPoseTopic"];
        VisualOdomTFManager* pVisualOdomTFManager = new VisualOdomTFManager(nh, file_path, tag_topic, tag_buff_size,
                                                                            camera_frame, odom_frame);

        pVisualOdomTFManager->intial_pose_subscriber_ = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>(initial_pose_topic, 1,
                                                               bind(&VisualOdomTFManager::InitialPoseCallback, pVisualOdomTFManager, _1));
        const string odom_source = fSettings["OdometrySource"];
        if(odom_source == "ORB-SLAM2"){
            cout<<"Odometry from ORB-SLAM2 . \n";
            const string orb_pose_topic = fSettings["ORBPoseTopic"];
            pVisualOdomTFManager->odom_subscriber_ = nh.subscribe<geometry_msgs::PoseStamped>(orb_pose_topic,5,
                                                    bind(&VisualOdomTFManager::OrbOdomPoseCallback,pVisualOdomTFManager,_1));
        }
        if(odom_source == "VINS-MONO"){
            cout<<"Odometry from Vins-Mono . \n";
            const string vins_pose_topic = fSettings["VinsPoseTopic"];
            pVisualOdomTFManager->odom_subscriber_ = nh.subscribe<nav_msgs::Odometry>(vins_pose_topic,5,
                                                    bind(&VisualOdomTFManager::VinsOdomPoseCallback,pVisualOdomTFManager,_1));
        }
        if(odom_source == "LIVOX"){

        }

    }

    ros::spin();

}
